#include "Protobot.h"

Protobot::Protobot()
{
	Servo turret;
}

void Protobot::begin()
{
	pinMode(4,OUTPUT);	// Left motor B
	pinMode(5,OUTPUT);	// Left motor pwm
	pinMode(6,OUTPUT);	// Right motor pwm
	pinMode(7,OUTPUT);	// Right motor A
	pinMode(8,OUTPUT);	// Left motor A
	pinMode(9,OUTPUT);	// Right motor B
	pinMode(13,OUTPUT);	// LED
	stop();	// writes all the motor pins low
	
	turret.attach(10);
	turret.write(90);
	stop();
}

void Protobot::leftMotor(int speed)
{
	speed = constrain(speed,-255,255);
	if(speed == 0)
	{
		digitalWrite(8,LOW);
		digitalWrite(4,LOW);
	}
	else if(speed > 0)
	{
		digitalWrite(8,HIGH);
		digitalWrite(4,LOW);
	}
	else if(speed < 0)
	{
		digitalWrite(8,LOW);
		digitalWrite(4,HIGH);
	}
	analogWrite(5,abs(speed));
}

void Protobot::rightMotor(int speed)
{
	speed = constrain(speed,-255,255);
	if(speed == 0)
	{
		digitalWrite(7,LOW);
		digitalWrite(9,LOW);
	}
	else if(speed > 0)
	{
		digitalWrite(7,HIGH);
		digitalWrite(9,LOW);
	}
	else if(speed < 0)
	{
		digitalWrite(7,LOW);
		digitalWrite(9,HIGH);
	}
	analogWrite(6,abs(speed));
}

void Protobot::turnLeft(int speed)
{
	leftMotor(-speed);
	rightMotor(speed);
}

void Protobot::turnRight(int speed)
{
	leftMotor(speed);
	rightMotor(-speed);
}

void Protobot::forward(int speed)
{
	leftMotor(speed);
	rightMotor(speed);
}

void Protobot::reverse(int speed)
{
	leftMotor(-speed);
	rightMotor(-speed);
}

void Protobot::stop()
{
	digitalWrite(6,LOW);
	digitalWrite(5,LOW);
}
	
void Protobot::turnServo(int angle)
{
	turret.write(constrain(angle,20,160));
}

byte Protobot::readSensor()
{
	long distance = 0;
	for(int i=0; i<100; i++)
	{
		distance += analogRead(A2);
		delay(1);
	}
	distance  /= 1500; // quick and dirty guesstimate for now
	distance = 50 - distance;
	// actual empirical distance conversion goes here
	
	return (byte)distance;
}
	
	
	
	
	